
#include "TaskSchema.h"

//#include "pwm.h"
//#include "adc.h"
#include "io.h"
#include "serial.h"
#include "Com.h"
//#include "spi.h"
//#include "i2c.h"
//#include "Controller.h"
//#include "SystemState.h"
//#include "FlightControl.h"
//#include "BatteryManagement.h"
#include "Meas.h"
#include "led.h"



// Perform an action every 1 ticks.
xTaskHandle Task_1ms_Handle;
unsigned char Task_1ms_ParameterToPass;
void Task_1ms(void* param)
{
    portTickType xLastWakeTime;
    const portTickType xFrequency = 1;

    // Initialise the xLastWakeTime variable with the current time.
    xLastWakeTime = xTaskGetTickCount();

    while (1)
    {
        // Wait for the next cycle.
        vTaskDelayUntil(&xLastWakeTime, xFrequency);

        // Perform action here.
        //Controller_MainFunction();
    }
}


// Perform an action every 10 ticks.
xTaskHandle Task_10ms_Handle;
unsigned char Task_10ms_ParameterToPass;
void Task_10ms(void* param)
{
    portTickType xLastWakeTime;
    const portTickType xFrequency = 10;

    // Initialise the xLastWakeTime variable with the current time.
    xLastWakeTime = xTaskGetTickCount();

    while (1)
    {
        // Wait for the next cycle.
        vTaskDelayUntil( &xLastWakeTime, xFrequency );

        // Perform action here.
        Serial_Rx_MainFunction();
        //SystemState_MainFunction();
        //FlightControl_MainFunction();
        Com_MainFunction();
        Serial_Tx_MainFunction();
    }
}



// Perform an action every 100 ticks.
xTaskHandle Task_100ms_Handle;
unsigned char Task_100ms_ParameterToPass;
void Task_100ms(void* param)
{
    portTickType xLastWakeTime;
    const portTickType xFrequency = 100;

    // Initialise the xLastWakeTime variable with the current time.
    xLastWakeTime = xTaskGetTickCount();

    while (1)
    {
        // Wait for the next cycle.
        vTaskDelayUntil( &xLastWakeTime, xFrequency );

        // Perform action here.
        Led_MainFunction();
        Measurement_MainFunction();
        //BatteryManagement_MainFunction();

        //only for debug. To be removed.
        //make RE1 change state every 500ms.
        {
            static uint8 TestCnt = 0;
            static uint8 Led = 0;

            if((TestCnt%5)==0)
            {
                if(Led == 0) {Led=1;}
                else {Led=0;}
                IO_Set(1, Led);
            }
            TestCnt++;
        }
    }
}


/* Function that creates all tasks. */
void StartTaskSchema(void)
{
    /* Create the task, storing the handle.  Note that the passed parameter
    ucParameterToPass must exist for the lifetime of the task, so in this
    case is declared static.  If it was just an an automatic stack variable
    it might no longer exist, or at least have been corrupted, by the time
    the new task attempts to access it. */
    xTaskCreate(Task_1ms,   "TASK_1MS",   STACK_SIZE_1MS,   &Task_1ms_ParameterToPass,   PRIO_1MS,   &Task_1ms_Handle);
    xTaskCreate(Task_10ms,  "TASK_10MS",  STACK_SIZE_10MS,  &Task_10ms_ParameterToPass,  PRIO_10MS,  &Task_10ms_Handle);
    xTaskCreate(Task_100ms, "TASK_100MS", STACK_SIZE_100MS, &Task_100ms_ParameterToPass, PRIO_100MS, &Task_100ms_Handle);
}



/* Function that deletes all tasks. */
void StopTaskSchema(void)
{
    vTaskDelete(&Task_1ms_Handle);
    vTaskDelete(&Task_10ms_Handle);
    vTaskDelete(&Task_100ms_Handle);
}



// Perform an action every 100 ticks.
xTaskHandle Task_Ini_Handle;
unsigned char Task_Ini_ParameterToPass;
void Task_Ini(void* param)
{
    while (1)
    {
        // Perform App initialization action here.
        Led_Ini();
        //BatteryManagement_Ini();
        //Measurement_Ini();
        //SystemState_Ini();
        //FlightControl_Ini();
        //Controller_Ini();

        // Perform BSW initialization action here.
        //PWM_Ini();
        //ADC_Ini();
        Com_Ini();
        Serial_Ini();
        IO_Ini();
        //SPI_Ini();
        //I2C_Ini();

        // Start task schema here
        StartTaskSchema();

        //suspend ourselves
        vTaskSuspend(NULL);
    }
}



void StartFreeRTOS(void)
{
    xTaskCreate(Task_Ini, "TASK_INI", STACK_SIZE_INI, &Task_Ini_ParameterToPass, PRIO_INI, &Task_Ini_Handle);

    // Start the real time kernel with preemption.
    vTaskStartScheduler();

    // Will not get here unless a task calls vTaskEndScheduler ()
    StopTaskSchema();

    while(1) {}
}



